//*************************************************************************************************
//************************ (C) 2001 - 2006, Eleven Engineering Incorporated ***********************
//*************************************************************************************************
//**
//**		Tabs:	This file looks best with tab stops set every 6 spaces.
//**
//*************************************************************************************************
//*************************************************************************************************
//**
//**          File:  XPD Debugger.asm
//**       Project:  XInc2 Library
//**       Created:  04 Jan 2003 by Ryan Northcott
//**       Revised:  08 Sep 2006 by Dan Beutel
//**
//**   Description:  A simple command line debugger for debugging XInc2 programs from a terminal
//**                 application connected to the XInc2 Program / Debug Port.  The debugger is
//**                 controlled by typing ASCII text commands in the terminal application.  This
//**                 program must be run on thread 0.  Thread 0 is not available to run other code
//**                 when using this debugger.  To use these routines in your project, you must
//**                 include the file "XPD_Debugger_Data.asm" in your "LongData.asm" file.
//**
//** Compatibility:  Requires XInA v4.4 or higher.
//**
//**    Disclaimer:  You may incorporate this sample source code into your program(s) without
//**                 restriction.  This sample source code has been provided "AS IS" and the
//**                 responsibility for its operation is yours.  You are not permitted to
//**                 redistribute this sample source code as "Eleven sample source code" after
//**                 having made changes.  If you are going to re-distribute the source, we require
//**                 that you make it clear in the source that the code was descended from Eleven
//**                 sample source code, but that you have made changes.
//**
//*************************************************************************************************
//*************************************************************************************************
//**
//** 	XPD_Debugger
//** 	
//** 	ReceiveCommand
//** 	ParseCommand
//** 	
//** 	CmdG
//** 	CmdS
//** 	CmdD
//** 	CmdDB
//** 	CmdE
//** 	CmdO
//** 	CmdW
//** 	CmdWM
//** 	CmdWP
//** 	CmdWC
//** 	CmdWR
//**
//** 	GetThreadByte
//** 	GetAddress
//** 	GetWord
//** 	StepOverBreakpoints
//** 	DisplayMachineState
//** 	DisplayCC
//**
//*************************************************************************************************
//*************************************************************************************************

///////////////////////////////////////////////////////////////////////////////
//
// DISCLAIMER:
//	The author makes no claim that this module is easy to use! It is simply
//	a minimal command set required to do the simplest debugging operations
//	which may be augmented by users as required.
//
// DIRECTIONS FOR USE:
//	Put the following statement in the code for Thread 0:
//		#include "XPD_Debugger.asm"
//	Make sure the following files are included:
//		#include "XInc2.h"
//		#include "XPD_Serial.asm"
//		#include "XPD_Echo.asm"
//		#include "XPD_Debugger_Data.asm"
//	
//	Compile and download the program to the target device.
//
//	While running the XInc2 Debug Application, create a new session.
//	Hit <Enter> to begin communication with the debugging code.  The
//	version number of the debug code is displayed.  For example:
//		XInc2 Debugger Lite (28 Jan 2002 Build)
//	To see a list of available commands, type '?' and hit <Enter>.
//
// RUNNING A PROGRAM:
//	Use the 'e' command to specify which threads should be enabled.  Enabled
//	threads will execute code when the go or step commands are used. The format
//	of the active thread byte may be in decimal, hex or binary.  Thread 0 is the
//	least significant bit and Thread 7 is the most significant bit.  For example, 
//	to enable threads 1 and 3, any of the following are equivalent:
//		>e 0b1010
//		>e 10
//		>e 0xA
//	NOTE: Thread 0 is always enabled whether or not the active thread byte
//	specifies thread 0 or not.
//
//	Use the 'g' (go) command to run all enabled threads.  The machine will
//	continue executing until a breakpoint is reached in the code of one or more
//	of the enabled threads.  (A breakpoint is defined as a 'bra @' instruction.)
//	To stop execution of your program following a 'g' command, hit <Enter>.
//	The message 'User Break' is displayed in the session window and all user
//	threads are halted.
//
//	Use the 's' (step) command to step all enabled threads for one instruction.
//
// DISPLAYING THREAD INFORMATION:
//	Many commands, after they complete execution, will display a table that
//	shows the current information for each thread.  This table includes the
//	enabled status, the PC, CC, and register set of each thread.
//
//	To see the contents of memory, use the 'db' (display block) command.  For
//	example, to display the 8 words starting at address 0x3ebf in rows of 4:
//		>db 0x3ebf 8 4
//		0x3ebf:  0x0000 0x0000 0x0000 0x0000
//		0x3ec3:  0x0000 0x0000 0x0000 0x0000
//
// INSERTING A BREAKPOINT:
//	Use the 'wm' (write memory) command to overwrite the current code word at
//	the desired breakpoint address.  For example, to insert a breakpoint
//	at address 0x034C, write the word 0x01FF at that address:
//		>wm 0x34c 0x1ff
//	Then use the 'g' command.  When the 0x01FF code word is encountered, 
//	Thread 0 halts execution of all user threads and displays a message
//	indicating which thread(s) have reached breakpoint(s).  For example:
//	"Breakpoint in Thread(s) 3" indicates that thread 3 has reached a
//	breakpoint.  This will also be indicated in the thread table by 'bkpt'
//	being displayed in the thread status row.  By invoking the 'g' or 's' 
//	command when stopped at a breakpoint, the debugger will advance the PC
//	to the next instruction after the breakpoint so that the code will
//	continue to execute.
//
// REMOVING A BREAKPOINT:
//	Use the 'write memory' command to overwrite the 0x01FF code word and
//	replace it with the original code word.
//
///////////////////////////////////////////////////////////////////////////////


//=============================================================================
// Input Params:	r0 = Bit field with bits set indicating the disabled threads
// Output Params:	None (Never Returns)
//-----------------------------------------------------------------------------
// Description:	This debugger routine runs continuously on thread 0.
//=============================================================================
XPD_Debugger:
			// store the currently running threads
				mov	r0, __RUN_THREADS__
				st	r0, EnabledThreads
				
				mov   sp, T0_SP
//			// Initialize the XPD Port
//				mov	r1, kXPD_BaudRate_115200 + kXPD_ClockLE_12MHz
//				jsr	r6, XPD_Configure
				
			// Initialize the LEDs
				jsr	r6, InitializeLEDs
				
				mov	r1, 0xFFFF
				jsr	r6, TurnOnLEDs		// Turn on all LEDs to indicate the debugger has loaded
			
				jsr	r6, XPD_ReadByte		// Wait for a key press on the computer's terminal
				
				mov	r1, 0xFFFF
				jsr	r6, TurnOffLEDs		// Turn off all LEDs once the user begins interacting with the debugger

			// assert: something received, show version
				mov	r1, MSG_VERSION
				jsr	r6, XPD_EchoString
				mov	r1, MSG_NEWLINE
				jsr	r6, XPD_EchoString
				jsr	r6, DisplayMachineState
				mov	r1, MSG_NEWLINE
				jsr	r6, XPD_EchoString
				
		DebugLoop:
				jsr	r6, ReceiveCommand
				jsr	r6, ParseCommand
				
				mov	r1, MSG_NEWLINE
				jsr	r6, XPD_EchoString
				
				bra	DebugLoop


//=============================================================================
// Input Params:	None
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Displays the command prompt and receives an incoming
//			command into a buffer.
//=============================================================================
ReceiveCommand:
				st	r6, sp, 0
				add	sp, sp, 1
				
		PromptCmd: // show prompt
				mov	r1, MSG_PROMPT
				jsr	r6, XPD_EchoString

		ReceiveCmd: // receive command
				mov	r5, BUFFER
				mov	r4, 0

			// ReceiveResponse:

		RR1:	// read response loop
				mov	r1, 0
				jsr	r6, XPD_ShiftInOut
				bis	r1, r1, kXPD_DataReceivedBit	// has byte arrived?
				bc	VC, RR1
				and	r1, r1, 0x00FF		// clear high byte

			// test most significant bit of low byte	
				bic	r1, r1, 8
				bc	VS, RR1

			// assert: high bit zero - have we got ASCII char?
				mov	r3, 0x20
				sub	r3, r1, r3
				bc	NS, RR2			// if < 0x20, test for end
			
			// assert: have ASCII char, save to buffer
				add	r4, r4, 1
				sub	r3, r4, kDebugBufferSize - 2	// check for max
				bc	NC, RR1
				st	r1, r5, 0			// have room, save to buffer
				add	r5, r5, 1			// increment pointer
				bra	RR1	

			// test for end
		RR2:		mov	r3, CR
				sub	r3, r1, r3
				bc	ZC, RR1

		RRDone: // done, mark end of line
				mov	r3, EOS
				st	r3, r5, 0
				
ReceiveCommand_END:
				sub	sp, sp, 1
				ld	r6, sp, 0
				jsr	r6, r6


//=============================================================================
// Input Params:	None
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Parses the command in the buffer.
//=============================================================================
ParseCommand:
				st	r6, sp, 0
				add	sp, sp, 1
				
			// get pointer to beginning of message
				mov	r5, BUFFER
				mov	r1, ERR_CMD

		PC_Loop:		
				ld	r3, r5, 0			// r3 = ASCII char
				bc	ZS, ParseCommand_END
				sub	r2, r3, '>'
				bc	ZS, PC_Next
				sub	r2, r3, ' '
				bc	ZS, PC_Next
				
			PC_Test0:
				sub	r2, r3, 'g'
				bc	ZC, PC_Test1
				jsr	r6, CmdG
				bra	ParseCommand_END
				
			PC_Test1:
				sub	r2, r3, 's'
				bc	ZC, PC_Test2
				jsr	r6, CmdS
				bra	ParseCommand_END
				
			PC_Test2:
				sub	r2, r3, 'd'
				bc	ZC, PC_Test3
				jsr	r6, CmdD
				bra	ParseCommand_END
				
			PC_Test3:
				sub	r2, r3, 'w'
				bc	ZC, PC_Test4
				jsr	r6, CmdW
				bra	ParseCommand_END
				
			PC_Test4:
				sub	r2, r3, 'e'
				bc	ZC, PC_Test5
				jsr	r6, CmdE
				bra	ParseCommand_END
				
			PC_Test5:
				sub	r2, r3, 'o'
				bc	ZC, PC_Test6
				jsr	r6, CmdO
				bra	ParseCommand_END
				
			PC_Test6:
				sub	r2, r3, '?'
				bc	ZC, PC_Error
				mov	r1, MSG_COMMANDS
				jsr	r6, XPD_EchoString
				bra	ParseCommand_END
				
			PC_Error:
				mov	r1, ERR_CMD
				jsr	r6, XPD_EchoString
				bra	ParseCommand_END

		PC_Next: // get next byte
				add	r5, r5, 1
				bra	PC_Loop
ParseCommand_END:
				sub	sp, sp, 1
				ld	r6, sp, 0
				jsr	r6, r6


//=============================================================================
// Input Params:	None
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Handles the "Go" command.  Will run the enabled threads until
//			a breakpoint is encountered or the <Enter> key is pressed.
//=============================================================================
CmdG:
				st	r6, sp, 0
				add	sp, sp, 1
				
				ld	r3, EnabledThreads
				//add	r1, r3, 0
				//jsr	r6, StepOverBreakpoints	// Takes the thread byte in r1
				
			// write thread byte
				outp	r3, SCUstop

			//////////////////////////////////////////////////////
			// GO LOOP - monitor for breakpoint or char received
		GoLoop:
				mov	r1, 0x0000
				jsr	r6, XPD_ShiftInOut
				bis	r1, r1, kXPD_DataReceivedBit	// Has a byte arrived?
				bc	VC, GoLoop1
				and	r1, r1, 0x00FF
				sub	r1, r1, CR		// Is the byte a Carriage Return?
				bc	ZC, GoLoop1
				
			// assert: byte received - inform of user break
				mov	r0, 0b11111110
				outp	r0, SCUstop
				mov	r1, MSG_USERBREAK
				jsr	r6, XPD_EchoString
				mov	r1, MSG_NEWLINE	// start new line
				jsr	r6, XPD_EchoString
				jsr	r6, DisplayMachineState
				bra	CmdG_END

		GoLoop1: // check for breakpoint
				inp	r1, SCUbkpt
				bc	ZS, GoLoop

			// assert: at least one thread is blocked - stop all
				mov	r0, 0b11111110
				outp	r0, SCUstop
				st	r1, SCU_PNTR

			// inform user
				mov	r1, MSG_BREAKPOINT
				jsr	r6, XPD_EchoString

			// show contents of r1
				ld	r1, SCU_PNTR
				jsr	r6, XPD_EchoSetBitList
				mov	r1, MSG_NEWLINE	// start new line
				jsr	r6, XPD_EchoString
				jsr	r6, DisplayMachineState
				bra	CmdG_END
				
CmdG_END:
				sub	sp, sp, 1
				ld	r6, sp, 0
				jsr	r6, r6


//=============================================================================
// Input Params:	None
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Handles the "Step" command.  Will execute the specified
//			number of instructions on each of the enabled threads.  If no
//			number of instructions is specified, a default of 1 is used.
//=============================================================================
CmdS:
				st	r6, sp, 0
				add	sp, sp, 1
				
				add	r5, r5, 1
				jsr	r6, GetAddress
				
				add	r1, r0, 0			// test return code
				bc	ZS, CmdS1
				mov	r2, 1
				st	r2, ADDRESS
				
		CmdS1:
				add	r1, r0, 0
				jsr	r6, XPD_EchoHex
				mov	r1, MSG_SPACE
				jsr	r6, XPD_EchoString
				ld	r1, ADDRESS
				jsr	r6, XPD_EchoHex

				ld	r3, EnabledThreads
				ld	r2, ADDRESS
				
		CmdS_Loop:
				bc	ZS, CmdS_END
				st	r1, sp, 0
				add	sp, sp, 1
				//add	r1, r3, 0
				//jsr	r6, StepOverBreakpoints	// Takes the thread byte in r1
				sub	sp, sp, 1
				ld	r1, sp, 0

			// step
				mov	r0, 0b11111110
				outp	r3, SCUstop			// activate selected
				outp	r0, SCUstop			// stop all
				sub	r2, r2, 1
				bra	CmdS_Loop
				
CmdS_END:
				mov	r1, MSG_NEWLINE	// start new line
				jsr	r6, XPD_EchoString
				jsr	r6, DisplayMachineState
				
				sub	sp, sp, 1
				ld	r6, sp, 0
				jsr	r6, r6


//=============================================================================
// Input Params:	None
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Handles commands starting with the letter "d".
//=============================================================================
CmdD:
				st	r6, sp, 0
				add	sp, sp, 1
				
			// check next char
				mov	r1, ERR_ADDRESS
				
				ld	r4, r5, 1
				sub	r2, r4, 'b'
				bc	ZC, CmdD_Error
				jsr	r6, CmdDB
				bra	CmdD_END
				
		CmdD_Error:
				mov	r1, ERR_CMD
				jsr	r6, XPD_EchoString
			
CmdD_END:
				sub	sp, sp, 1
				ld	r6, sp, 0
				jsr	r6, r6


//=============================================================================
// Input Params:	None
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Handles the "Display Block" command.
//=============================================================================
CmdDB:
				st	r6, sp, 0
				add	sp, sp, 1
				
			// assert: display block command - get start address
				add	r5, r5, 2
				mov	r4, ADDRESS
				jsr	r6, GetAddress
				add	r0, r0, 0			// test return code
				bc	ZS, CmdDB_Continue
				jsr	r6, XPD_EchoString
				bra	CmdDB_END

		CmdDB_Continue:
			 // get number of bytes to display
				mov	r4, WORD_VALUE
				jsr	r6, GetWord
				add	r0, r0, 0			// test return code
				bc	ZS, CmdDB0
				mov	r0, 24			// default of 24 if no value given
				st	r0, WORD_VALUE

		CmdDB0:
			// get number of blocks per line
				mov	r4, NUM_PER_LINE
				jsr	r6, GetWord
				add	r0, r0, 0			// test return code
				bc	ZS, CmdDB1
				mov	r0, 8				// default of 8 if no value given
				st	r0, NUM_PER_LINE

		CmdDB1: // display memory block - 8 words per line
				ld	r5, ADDRESS			// r5 = address of word
				ld	r4, WORD_VALUE		// r4 = number of words total
				ld	r3, NUM_PER_LINE		// r3 = number of blocks per line

				mov	r1, MSG_NEWLINE
				jsr	r6, XPD_EchoString
			
		CmdDB2:
				ld	r3, NUM_PER_LINE		// r3 = number of blocks per line
				add	r1, r5, 0			// display memory address
				jsr	r6, XPD_EchoHex		// in hex
				mov	r1, MSG_COLON
				jsr	r6, XPD_EchoString
				mov	r1, MSG_SPACE
				jsr	r6, XPD_EchoString

		CmdDBLoop:
				ld	r1, r5, 0
				jsr	r6, XPD_EchoHex
				mov	r1, ' '
				jsr	r6, XPD_WriteByte
				add	r5, r5, 1			// increment address
				sub	r4, r4, 1			// decrement total
				bc	ZS, CmdDB_END
				sub	r3, r3, 1			// decrement words on this line
				bc	ZC, CmdDBLoop
				mov	r1, MSG_NEWLINE		// start new line
				jsr	r6, XPD_EchoString
				bra	CmdDB2
				
CmdDB_END:
				sub	sp, sp, 1
				ld	r6, sp, 0
				jsr	r6, r6


//=============================================================================
// Input Params:	None
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Handles the "Enable Threads" command.
//=============================================================================
CmdE:
				st	r6, sp, 0
				add	sp, sp, 1
				
				add	r5, r5, 1
				jsr	r6, GetThreadByte
				add	r0, r0, 0			// test r0
				bc	ZC, CmdE_Error
				st	r3, EnabledThreads
				
				mov	r1, MSG_NEWLINE		// start new line
				jsr	r6, XPD_EchoString
				jsr	r6, DisplayMachineState
				
				bra	CmdE_END
				
		CmdE_Error:
				mov	r1, ERR_THREAD
				jsr	r6, XPD_EchoString

CmdE_END:
				sub	sp, sp, 1
				ld	r6, sp, 0
				jsr	r6, r6


//=============================================================================
// Input Params:	None
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Handles the "Output" command.
//=============================================================================
CmdO:
				st	r6, sp, 0
				add	sp, sp, 1
				
				// <<<<<<<<<<<<<<<<<<<<<<< Add your debug code here
				
CmdO_END:
				sub	sp, sp, 1
				ld	r6, sp, 0
				jsr	r6, r6


//=============================================================================
// Input Params:	None
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Handles commands starting with the letter "w".
//=============================================================================
CmdW:
				st	r6, sp, 0
				add	sp, sp, 1
				
				ld	r4, r5, 1			// r4 = char
				
		CmdW_Test0:
				sub	r3, r4, 'm'
				bc	ZC, CmdW_Test1
				jsr	r6, CmdWM
				bra	CmdW_END
				
		CmdW_Test1:
				sub	r3, r4, 'p'
				bc	ZC, CmdW_Test2
				jsr	r6, CmdWP
				bra	CmdW_END
		
		CmdW_Test2:
				sub	r3, r4, 'c'
				bc	ZC, CmdW_Test3
				jsr	r6, CmdWC
				bra	CmdW_END
		
		CmdW_Test3:
				sub	r3, r4, 'r'
				bc	ZC, CmdW_Error
				jsr	r6, CmdWR
				bra	CmdW_END
				
		CmdW_Error:
				mov	r1, ERR_CMD
				jsr	r6, XPD_EchoString

CmdW_END:
				sub	sp, sp, 1
				ld	r6, sp, 0
				jsr	r6, r6


//=============================================================================
// Input Params:	None
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Handles the "Write Memory" command.
//=============================================================================
CmdWM:
				st	r6, sp, 0
				add	sp, sp, 1
				
			// assert: write memory - get address
				add	r5, r5, 2
				jsr	r6, GetAddress
				add	r1, r0, 0			// test return code
				bc	ZS, CmdWM_Continue0
				bra	CmdWM_Error

		CmdWM_Continue0:
			// get value to write
				mov	r4, WORD_VALUE
				jsr	r6, GetWord
				add	r1, r0, 0
				bc	ZS, CmdWM_Continue1
				bra	CmdWM_Error
				
		CmdWM_Continue1:
				ld	r3, ADDRESS
				ld	r1, WORD_VALUE
				st	r1, r3, 0
				mov	r1, MSG_DONE
				bra	CmdWM_END
				
		CmdWM_Error:
				jsr	r6, XPD_EchoString

CmdWM_END:
				sub	sp, sp, 1
				ld	r6, sp, 0
				jsr	r6, r6


//=============================================================================
// Input Params:	None
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Handles the "Write Program Counter" command.
//=============================================================================
CmdWP:
				st	r6, sp, 0
				add	sp, sp, 1
				
			// get thread number
				mov	r1, ERR_THREAD
				ld	r4, r5, 3
				sub	r4, r4, '0'	// r4 = thread number
				bc	NS, CmdWP_Error
				sub	r0, r4, 8
				bc	NC, CmdWP_Error

			// assert: have valid thread number
				rol	r4, r4, 3		// shift left 3 places
				st	r4, SCU_PNTR

			// get program counter value
				add	r5, r5, 5
				mov	r4, WORD_VALUE
				jsr	r6, GetWord
				add	r1, r0, 0		// test return code
				bc	ZC, CmdWP_Error

			// write pc
				ld	r1, SCU_PNTR
				ld	r0, WORD_VALUE
				outp	r1, SCUpntr
				outp	r0, SCUpc
				
				mov	r1, MSG_NEWLINE	// start new line
				jsr	r6, XPD_EchoString
				jsr	r6, DisplayMachineState
				bra	CmdWP_END
				
		CmdWP_Error:
				jsr	r6, XPD_EchoString

CmdWP_END:
				sub	sp, sp, 1
				ld	r6, sp, 0
				jsr	r6, r6


//=============================================================================
// Input Params:	None
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Handles the "Write Condition Code" command.
//=============================================================================
CmdWC:
				st	r6, sp, 0
				add	sp, sp, 1
				
	 		// get thread number
				mov	r1, ERR_THREAD
				ld	r4, r5, 3
				sub	r4, r4, '0'		// r4 = thread number
				bc	NS, CmdWC_Error
				sub	r0, r4, 8
				bc	NC, CmdWC_Error

			// assert: have valid thread number
				rol	r4, r4, 3		// shift left 3 places
				st	r4, SCU_PNTR

			// get condition code value
				add	r5, r5, 5
				mov	r4, WORD_VALUE
				jsr	r6, GetWord
				add	r1, r0, 0		// test return code
				bc	ZC, CmdWC_Error

			// write cc
				ld	r1, SCU_PNTR
				ld	r0, WORD_VALUE
				outp	r1, SCUpntr
				outp	r0, SCUcc
				
				mov	r1, MSG_NEWLINE	// start new line
				jsr	r6, XPD_EchoString
				jsr	r6, DisplayMachineState
				bra	CmdWC_END
				
		CmdWC_Error:
				jsr	r6, XPD_EchoString

CmdWC_END:
				sub	sp, sp, 1
				ld	r6, sp, 0
				jsr	r6, r6


//=============================================================================
// Input Params:	None
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Handles the "Write Register" command.
//=============================================================================
CmdWR:
				st	r6, sp, 0
				add	sp, sp, 1
				
			 // get thread number
				mov	r1, ERR_THREAD
				ld	r4, r5, 3
				sub	r4, r4, '0'		// r4 = thread number
				bc	NS, CmdWR_Error
				sub	r0, r4, 8
				bc	NC, CmdWR_Error

			// assert: have valid thread number
				rol	r4, r4, 3		// shift left 3 places
				st	r4, SCU_PNTR
				
				ld	r3, r5, 5
			// assert: write register
				mov	r1, ERR_REG
				sub	r3, r3, '0'		// r3 = register number
				bc	NS, CmdWR_Error
				sub	r2, r3, 8
				bc	NC, CmdWR_Error

			// assert: have valid register number
				add	r4, r4, r3
				st	r4, SCU_PNTR

			// get value
				add	r5, r5, 6
				mov	r4, WORD_VALUE
				jsr	r6, GetWord
				add	r1, r0, 0		// test return code
				bc	ZC, CmdWR_Error

			// write register value
				ld	r1, SCU_PNTR
				ld	r0, WORD_VALUE
				outp	r1, SCUpntr
				outp	r0, SCUreg
				
				mov	r1, MSG_NEWLINE	// start new line
				jsr	r6, XPD_EchoString
				jsr	r6, DisplayMachineState
				bra	CmdWR_END
				
		CmdWR_Error:
				jsr	r6, XPD_EchoString

CmdWR_END:
				sub	sp, sp, 1
				ld	r6, sp, 0
				jsr	r6, r6

//=============================================================================
// Input Params:	r5 = Pointer to Buffer
// Output Params:	r0 = 0 if successful, non-zero otherwise
//			r3 = Thread Byte
//-----------------------------------------------------------------------------
// Description:	Calls GetWord to get the value, ensures that at least one 
//			thread is active (other than thread 0), and ensures that
//			thread 0 remains active.
//=============================================================================
GetThreadByte:
				st	r2, sp, 0
				st	r4, sp, 1
				st	r6, sp, 2
				add	sp, sp, 3

				mov	r4, THREAD_BYTE
				jsr	r6, GetWord
				add	r0, r0, 0		// test return code
				bc	ZC, GTBError

				xor	r3, r3, 0xFFFF	// invert bits
				and	r3, r3, 0x00FE	// clear bits
				mov	r0, 0			// clear return code
				bra	GetThreadByte_END

		GTBError:
				mov	r0, ERR_THREADBYTE

GetThreadByte_END:
				sub	sp, sp, 3
				ld	r2, sp, 0
				ld	r4, sp, 1
				ld	r6, sp, 2
				jsr	r6, r6


//=============================================================================
// Input Params:	r5 = Pointer to Buffer
// Output Params:	r0 = 0 if successful, non-zero otherwise
//			r3 = Read Address
//			r5 = Incremented Buffer Pointer
//-----------------------------------------------------------------------------
// Description:	Scans the buffer for a 16-bit value (dec, hex, or binary)
//=============================================================================
GetAddress:
				st	r6, sp, 0
				add	sp, sp, 1

				mov	r4, ADDRESS
				jsr	r6, GetWord

GetAddress_END:
				sub	sp, sp, 1
				ld	r6, sp, 0
				jsr	r6, r6


//=============================================================================
// Input Params:	r4 = Pointer to Result Destination
//			r5 = Pointer to Buffer
// Output Params:	r0 = 0 if successful, non-zero otherwise
//			r3 = Read Word
//			r5 = Incremented Buffer Pointer
//-----------------------------------------------------------------------------
// Description:	Scans the buffer for a 16-bit value (dec, hex, or binary)
//=============================================================================
GetWord:
				st	r1, sp, 0
				st	r2, sp, 1
				st	r4, sp, 2
				st	r6, sp, 3
				add	sp, sp, 4

		GW1: // skip leading blanks
				ld	r3, r5, 0		// r3 = current char
				bc	ZS, GWError		// error if end of buffer
				sub	r2, r3, ' '
				bc	ZS, GW2
				bra	GW3

		GW2: // get next byte
				add	r5, r5, 1
				bra	GW1

		GW3: // assert: not blank or end of buffer - check for ASCII zero
				sub	r1, r3, '0'		// r1 = numeric value
				bc	NS, GWError		// error if less than '0'
				bc	ZC, GW4		// if !'0', skip to decimal

			// assert: ASCII zero
			// check next char for 'b' or 'x'
				ld	r3, r5, 1
				sub	r2, r3, 'b'
				bc	ZS, GWB
				sub	r2, r3, 'B'
				bc	ZS, GWB
				sub	r2, r3, 'x'
				bc	ZS, GWX
				sub	r2, r3, 'X'
				bc	ZS, GWX

		GW4: // assert: decimal number
				sub	r2, r1, 10
				bc	NC, GWError		// error if >= 10
				st	r1, r4, 0		// save to dest

		GWD: // loop over decimal digits
				add	r5, r5, 1		// get next char
				ld	r3, r5, 0		// end of input buffer?
				bc	ZS, GWOK
				sub	r2, r3, ' '		// delimiter?
				bc	ZS, GWOK
				sub	r1, r3, '0'
				bc	NS, GWError		// error if < '0'
				sub	r2, r1, 10
				bc	NC, GWError		// error if >= 10
			
			// load previous value and multiply by 10 = ((2 x N) + (8 x N))
			// - first multiply by 2 and save
			// - then multiply by 4 and add to first
				ld	r3, r4, 0
				rol	r3, r3, 1
				st	r3, r4, 0
				ld	r2, r4, 0
				rol	r3, r3, 2
				add	r0, r2, r3		// r0 = 10 * prev char
				add	r0, r1, r0		// r0 = cur char + 10 * prev char
				st	r0, r4, 0		// save to destination
				bra	GWD

		GWB: // binary digits
				add	r5, r5, 1		// advance pointer
				mov	r0, 0xFFFF		// set count to -1
			
		GWB1:		st	r1, r4, 0		// save r1
				add	r5, r5, 1		// advance pointer
				add	r0, r0, 1		// increment count
				ld	r3, r5, 0		// r3 = cur char
				bc	ZS, GWB2		// end of buffer?
				sub	r2, r3, ' '		// delimiter?
				bc	ZS, GWB2
				sub	r1, r3, '0'		// r1 = numeric value
				bc	NS, GWError		// error if < '0'
				sub	r2, r1, 2
				bc	NC, GWError		// error if > 1
			
			// shift previous value left and OR current value
				ld	r2, r4, 0
				rol	r2, r2, 1
				ior	r1, r2, r1
				bra	GWB1

		GWB2: // check at least 1 digit and not more than 16
				add	r2, r0, 0
				bc	LE0, GWError		// error if <= 0
				sub	r2, r0, 17
				bc	NC, GWError		// error if >= 17
				bra	GWOK

		GWX: // hex digits
				add	r5, r5, 1		// advance pointer
				mov	r0, 0xFFFF		// set count to -1
			
		GWX1:		st	r1, r4, 0		// save r1
				add	r5, r5, 1		// advance pointer
				add	r0, r0, 1		// increment count
				ld	r3, r5, 0		// r3 = cur char
				bc	ZS, GWX4		// end of buffer?
				sub	r2, r3, ' '		// delimiter?
				bc	ZS, GWX4
				sub	r1, r3, '0'		// r1 = numeric value
				bc	NS, GWError		// error if < '0'
				sub	r2, r1, 10
				bc	NS, GWX3		// ok if < 10

			// check for 'A' to 'F' or 'a' to 'f'
				sub	r1, r3, 'A'		// r1 = numeric value
				bc	NS, GWError		// error if < 'A'
				sub	r2, r3, 'G'		// ok if < 'G'
				bc	NS, GWX2
				sub	r1, r3, 'a'		// r1 = numeric value
				bc	NS, GWError		// error if < 'a'
				sub	r2, r3, 'g'
				bc	NC, GWError		// error if >= 'g'

		GWX2: // add 10
				add	r1, r1, 10
				
		GWX3: // shift previous value left 4 places and OR current value
				ld	r2, r4, 0
				rol	r2, r2, 4
				ior	r1, r2, r1
				bra	GWX1

		GWX4: // check at least one digit and not more than 4
				add	r2, r0, 0
				bc	LE0, GWError		// error if <= 0
				sub	r2, r0, 5
				bc	NS, GWOK		// no error if < 5
			// fall thru for error

		GWError:
				mov	r0, ERR_WORD
				bra	GetWord_END

		GWOK:		mov	r0, 0			// clear return code
				ld	r3, r4, 0		// place value in r3

GetWord_END:
				sub	sp, sp, 4
				ld	r1, sp, 0
				ld	r2, sp, 1
				ld	r4, sp, 2
				ld	r6, sp, 3
				
				jsr	r6, r6


//=============================================================================
// Input Params:	r1 = Enabled Threads Byte (0 = Enabled, 1 = Disabled)
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Advances the PC of each enabled thread that is currently at a
//			breakpoint.
//=============================================================================
StepOverBreakpoints:
				st	r0, sp, 0
				st	r1, sp, 1
				st	r2, sp, 2
				st	r3, sp, 3
				st	r4, sp, 4
				st	r5, sp, 5
				st	r6, sp, 6
				add	sp, sp, 7
				
				mov	r0, 1	// i = 1
				mov	r2, 1

		StepOverBreakpoints_Loop:
				rol	r3, r2, r0
				and	r3, r3, r1
				bc	ZC, StepOverBreakpoints_LoopEnd

				rol	r3, r0, 3
				outp	r3, SCUpntr
				mov	r3, 0			// Stall for 1 tick
				inp	r3, SCUpc		// Read the PC
				ld	r4, r3, 0		// r4 = instruction at r3
				sub	r4, r4, 0x01FF	// Test for breakpoint (bra to @) instruction
				bc	ZC, StepOverBreakpoints_LoopEnd
				
				add	r3, r3, 1		// PC++ (skip over the breakpoint)
				outp	r3, SCUpc		// Write the PC

		StepOverBreakpoints_LoopEnd:
				add	r0, r0, 1		// i++
				sub	r3, r0, 8
				bc	ZC, StepOverBreakpoints_Loop
				
				sub	sp, sp, 7
				ld	r0, sp, 0
				ld	r1, sp, 1
				ld	r2, sp, 2
				ld	r3, sp, 3
				ld	r4, sp, 4
				ld	r5, sp, 5
				ld	r6, sp, 6
				jsr	r6, r6


//=============================================================================
// Input Params:	None
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Outputs the PC, CC, and all registers of each thread in table
//			format.
//=============================================================================
DisplayMachineState:
				st	r0, sp, 0
				st	r1, sp, 1
				st	r2, sp, 2
				st	r3, sp, 3
				st	r4, sp, 4
				st	r5, sp, 5
				st	r6, sp, 6
				add	sp, sp, 7
				
				mov	r1, MSG_NEWLINE
				jsr	r6, XPD_EchoString
				
				mov	r1, MSG_TAB_HEADER
				jsr	r6, XPD_EchoString
				
			// -------------------
				
				mov	r1, MSG_3SPACES
				jsr	r6, XPD_EchoString
				
				mov	r2, 7
				ld	r3, EnabledThreads
				mov	r5, 15
		DMS_StatusLoop:
				sub	r4, r5, r2
				rol	r4, r3, r4
				bc	NC, DMS_StatusLoop_On
				mov	r1, MSG_OFF
				jsr	r6, XPD_EchoString
				mov	r1, MSG_5SPACES
				jsr	r6, XPD_EchoString
				bra	DMS_StatusLoop_next
			
		DMS_StatusLoop_On:
				rol	r1, r2, 3
				outp	r1, SCUpntr
				mov	r1, 0			// stall 1 tick
				inp	r1, SCUpc		// Read the PC
				ld	r1, r1, 0		// r1 = instruction at r1
				sub	r1, r1, 0x01FF	// Test for breakpoint (bra to @) instruction
				bc	ZS, DMS_StatusLoop_Bkpt
				
				mov	r1, MSG_ON
				jsr	r6, XPD_EchoString
				mov	r1, MSG_6SPACES
				jsr	r6, XPD_EchoString
				bra	DMS_StatusLoop_next
				
		DMS_StatusLoop_Bkpt:
				mov	r1, MSG_BKPT
				jsr	r6, XPD_EchoString
				mov	r1, MSG_4SPACES
				jsr	r6, XPD_EchoString
			
		DMS_StatusLoop_next:
				sub	r2, r2, 1
				bc	ZC, DMS_StatusLoop
				
				mov	r1, MSG_DEBUG
				jsr	r6, XPD_EchoString
				
				mov	r1, MSG_NEWLINE
				jsr	r6, XPD_EchoString
			
			// -------------------
			
				mov	r1, MSG_PC
				jsr	r6, XPD_EchoString
				
				mov	r2, 7
		DMS_PCLoop:
				rol	r4, r2, 3		// shift left 3 places
				outp	r4, SCUpntr
				mov	r1, 0			// stall 1 tick
				inp	r1, SCUpc		// r1 = pc
				
				st	r2, sp, 0
				add	sp, sp, 1
				jsr	r6, XPD_EchoHex
				sub	sp, sp, 1
				ld	r2, sp, 0
				
				mov	r1, MSG_2SPACES
				st	r2, sp, 0
				add	sp, sp, 1
				jsr	r6, XPD_EchoString
				sub	sp, sp, 1
				ld	r2, sp, 0

				sub	r2, r2, 1
				bc	NC, DMS_PCLoop
				
				mov	r1, MSG_NEWLINE
				jsr	r6, XPD_EchoString
			
			// -------------------
			
				mov	r1, MSG_CC
				jsr	r6, XPD_EchoString
				
				mov	r2, 7
		DMS_CCLoop:
				rol	r4, r2, 3		// shift left 3 places
				outp	r4, SCUpntr
				mov	r1, 0			// stall 1 tick
				inp	r0, SCUcc		// r0 = cc
				jsr	r6, DisplayCC
				
				mov	r1, MSG_4SPACES
				jsr	r6, XPD_EchoString

				sub	r2, r2, 1
				bc	NC, DMS_CCLoop
				
				mov	r1, MSG_NEWLINE
				jsr	r6, XPD_EchoString
			
			// -------------------
			
				mov	r0, 0
		DMS_RegLoop0:
				mov	r1, MSG_R
				jsr	r6, XPD_EchoString

				add	r1, r0, '0'
				jsr	r6, XPD_WriteByte
				
				mov	r1, MSG_SPACE
				jsr	r6, XPD_EchoString
				
				mov	r2, 7
		DMS_RegLoop1:
				rol	r4, r2, 3		// shift left 3 places
				add	r5, r4, r0
				outp	r5, SCUpntr
				inp	r1, SCUreg		// r1 = reg
				jsr	r6, XPD_EchoHex
				
				mov	r1, MSG_2SPACES
				jsr	r6, XPD_EchoString

				sub	r2, r2, 1
				bc	NC, DMS_RegLoop1
				
				mov	r1, MSG_NEWLINE
				jsr	r6, XPD_EchoString
				
				add	r0, r0, 1
				sub	r5, r0, 8
				bc	ZC, DMS_RegLoop0
	
DisplayMachineState_END:
				sub	sp, sp, 7
				ld	r0, sp, 0
				ld	r1, sp, 1
				ld	r2, sp, 2
				ld	r3, sp, 3
				ld	r4, sp, 4
				ld	r5, sp, 5
				ld	r6, sp, 6
				jsr	r6, r6


//=============================================================================
// Input Params:	r0 = The value of the CC to display.
// Output Params:	None
//-----------------------------------------------------------------------------
// Description:	Outputs a CC value in ASCII representation by outputing a N,
//			Z, V, or C if that bit is set in the CC value passed in. If
//			a bit is not set, a dash is displayed instead.
//=============================================================================
DisplayCC:
				st	r1, sp, 0
				st	r6, sp, 1
				add	sp, sp, 2
				
				bic	r1, r0, 3
				bc	VC, DCC_NoNBit
				mov	r1, MSG_N
				jsr	r6, XPD_EchoString
				bra	DCC_TestZBit
		DCC_NoNBit:
				mov	r1, MSG_DASH
				jsr	r6, XPD_EchoString

		DCC_TestZBit:
				bic	r1, r0, 2
				bc	VC, DCC_NoZBit
				mov	r1, MSG_Z
				jsr	r6, XPD_EchoString
				bra	DCC_TestVBit
		DCC_NoZBit:
				mov	r1, MSG_DASH
				jsr	r6, XPD_EchoString

		DCC_TestVBit:
				bic	r1, r0, 1
				bc	VC, DCC_NoVBit
				mov	r1, MSG_V
				jsr	r6, XPD_EchoString
				bra	DCC_TestCBit
		DCC_NoVBit:
				mov	r1, MSG_DASH
				jsr	r6, XPD_EchoString
			
		DCC_TestCBit:
				bic	r1, r0, 0
				bc	VC, DCC_NoCBit
				mov	r1, MSG_C
				jsr	r6, XPD_EchoString
				bra	DisplayCC_END
		DCC_NoCBit:
				mov	r1, MSG_DASH
				jsr	r6, XPD_EchoString

DisplayCC_END:
				sub	sp, sp, 2
				ld	r1, sp, 0
				ld	r6, sp, 1
				jsr	r6, r6


#include "XPD Echo.asm"
#include "LEDs.asm"